/** 
 * @file AppRoutines.h
 * 
 * @detail Description : 
 * Sensor Calibration Header 
 * 
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
 * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 **/

#include "AppRoutines.h"

/**
* @brief <b>Function Name</b>:     : calibrateSensors                                               
* @brief  <b>Description</b>: This function initializes the offset values for 
* each motion sensor. This values can be calculated using the MSP430 Calibration
* GUI. 
* @param Input Parameters: None                                                  
* @return Return Values: None                                                    
**/ 
void calibrateSensors(void)
{
    // X offset
    sen_offset.accel_offset[0]     = 10.75; 
    // Y offset
    sen_offset.accel_offset[1]     = 1; 
    // Z offset
    sen_offset.accel_offset[2]     = -5; 

    // X offset    
    sen_offset.gyro_offset[0]      = 0; 
    // Y offset
    sen_offset.gyro_offset[1]      = 0.5; 
    // Z offset    
    sen_offset.gyro_offset[2]      = -0.5; 

    // X offset    
    sen_offset.magnetom_offset[0]  = 49; 
    // Y offset    
    sen_offset.magnetom_offset[1]  = -136;  
    // Z offset    
    sen_offset.magnetom_offset[2]  = -130; 
    
    
    sen_offset.magnetom_XY_Theta   = ToRad(0);
    sen_offset.magnetom_XY_Scale   = 1.0;
    
    sen_offset.magnetom_YZ_Theta   = ToRad(0);
    sen_offset.magnetom_YZ_Scale   = 1;
    
}

/**
* @brief <b>Function Name</b>:     : StatusLEDToggle                                               
* @brief  <b>Description</b>: This function toggles the three LEDS in the board
* every 20 times is called. 
* @param Input Parameters: None                                                  
* @return Return Values: None                                                    
**/ 
void StatusLEDToggle(void)
{
  static uint8_t counter = 0;
  static boolean_t state = false_state;
  
  counter++;
  if (counter > 20)
  {
    counter=0;
    if(state)
    {
      P1OUT &= ~(BIT2+BIT3+BIT4);
      state = false_state;
    }
    else
    {
      P1OUT |= (BIT2+BIT3+BIT4);
      state = true_state;
    }
 
  }
}






